#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
from time import sleep
from sg805_driver.msg import FastPoint



class FastPointDemo:
    def __init__(self):
        rospy.init_node('fastPointCommander', anonymous = False )
        pub = rospy.Publisher("/sg805/fastPoint", FastPoint, queue_size=10)
        fastPointMsg = FastPoint()

        
        #根据功能编号进行调用，参考 sg805RobotRos中 fastPointCB回调函数的内容。
        fastPointMsg.Mode = 0        
        fastPointMsg.duration = 4000
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024) 

        print("发送")
        pub.publish(fastPointMsg)


        
        fastPointMsg.duration = 4000
        fastPointMsg.Position.append(-10240)
        fastPointMsg.Position.append(-10240)
        fastPointMsg.Position.append(-10240)
        fastPointMsg.Position.append(-10240)
        fastPointMsg.Position.append(-10240)
        fastPointMsg.Position.append(-10240) 
        print("发送")
        pub.publish(fastPointMsg)
        sleep(10)
        

        while(not rospy.is_shutdown()):
            fastPointMsg.duration = 5000
            fastPointMsg.Position.append(-10240)
            fastPointMsg.Position.append(-10240)
            fastPointMsg.Position.append(-10240)
            fastPointMsg.Position.append(-10240)
            fastPointMsg.Position.append(-10240)
            fastPointMsg.Position.append(-10240) 
            print("发送")
            pub.publish(fastPointMsg)
            fastPointMsg.Position.clear()
            sleep(10)


            fastPointMsg.duration = 5000
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024) 
            print("发送")
            pub.publish(fastPointMsg)
            fastPointMsg.Position.clear()
            sleep(10)

            fastPointMsg.duration = 5000
            fastPointMsg.Position.append(10240)
            fastPointMsg.Position.append(10240)
            fastPointMsg.Position.append(10240)
            fastPointMsg.Position.append(10240)
            fastPointMsg.Position.append(10240)
            fastPointMsg.Position.append(10240) 
            print("发送")
            pub.publish(fastPointMsg)
            fastPointMsg.Position.clear()
            sleep(10)

            fastPointMsg.duration = 5000
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024)
            fastPointMsg.Position.append(-1024) 
            print("发送")
            pub.publish(fastPointMsg)
            fastPointMsg.Position.clear()
            sleep(10)

if __name__ == '__main__':
    try:
        handle = FastPointDemo()
    except rospy.ROSInterruptException:
        pass